#include "pc_interaction.h"
#include "string.h"
#include "stdio.h"
#include "usart.h"
#include "can_cfg.h"
#include "pneumatic_muscle_control.h"

extern UART_HandleTypeDef huart3;
extern DMA_HandleTypeDef hdma_usart3_rx;
extern osEventFlagsId_t Tx_Gui_eventHandle;
extern float real_r_trachea[8];
extern float real_r_trachea_rc_filter_new[pp_num + 1];
extern int16_t Joint1_real_callback_ang;
extern int16_t Joint2_real_callback_ang;
extern uint8_t ppv_rev[ppv_data_len];
extern float angle_x;
float fAcc[3],fGyro[3],fAngle[3];
float pitch,roll,yaw;
float elbow_fAcc[3],elbow_fGyro[3],elbow_fAngle[3];
float elbow_pitch,elbow_roll,elbow_yaw;

unsigned char send_data_pack[200];
uint8_t PC_OR_HWT901_Ctr_Flag = 1;  //0 HWT901控制 1 PC控制
uint8_t tendon_Ctr_Flag = 1; //0 肌腱不加入控制 1 肌腱加入控制
uint8_t huart3_rev_data[79];
uint8_t send_gui_flag = 0;
uint64_t temp_test_count = 0;

uint8_t ctr_cmd = 0x00;
uint8_t ctr_ID = 100;
uint8_t motor_type = 0;
uint16_t ctr_abs_ang = 0;
uint8_t Simulate_Human_Mode = 0;
int16_t rev_pc_data_length = 0;
int16_t polhemux_data_length = 0;

uint64_t test_idle_uart3_count = 0;
extern float target_angle_x;
extern float elbow_angle;
extern float wrist_trachea_diff;

void pc_data_deal();
void ToPC_dataPack();

void GUI_communication_init(void)
{
    __HAL_UART_ENABLE_IT(&huart3, UART_IT_TC);
    ToPC_dataPack();
    __HAL_UART_ENABLE_IT(&huart3, UART_FLAG_IDLE);
    HAL_UART_Receive_DMA(&huart3, huart3_rev_data, sizeof(huart3_rev_data));
}

void ToPC_dataPack()
{
  memset(send_data_pack, ToGUI_DataLEN, 0);
  unsigned char index = 0;
  send_data_pack[index++] = 0xAA;
  send_data_pack[index++] = 0x01;

  adc_stop();
  PPV_Cal();
  send_data_pack[index++] = *(unsigned long int *)&Joint1_real_callback_ang >> 24 & 0xFF;
  send_data_pack[index++] = *(unsigned long int *)&Joint1_real_callback_ang >> 16 & 0xFF;
  send_data_pack[index++] = *(unsigned long int *)&Joint1_real_callback_ang >> 8 & 0xFF;
  send_data_pack[index++] = *(unsigned long int *)&Joint1_real_callback_ang & 0xFF;

  send_data_pack[index++] = *(unsigned long int *)&Joint2_real_callback_ang >> 24 & 0xFF;
  send_data_pack[index++] = *(unsigned long int *)&Joint2_real_callback_ang >> 16 & 0xFF;
  send_data_pack[index++] = *(unsigned long int *)&Joint2_real_callback_ang >> 8 & 0xFF;
  send_data_pack[index++] = *(unsigned long int *)&Joint2_real_callback_ang & 0xFF;

  // 反馈气压显示 
  send_data_pack[index++] = *(unsigned long int *)&real_r_trachea_rc_filter_new[1] >> 24 & 0xFF;
  send_data_pack[index++] = *(unsigned long int *)&real_r_trachea_rc_filter_new[1] >> 16 & 0xFF;
  send_data_pack[index++] = *(unsigned long int *)&real_r_trachea_rc_filter_new[1] >> 8 & 0xFF;
  send_data_pack[index++] = *(unsigned long int *)&real_r_trachea_rc_filter_new[1] & 0xFF;

  send_data_pack[index++] = *(unsigned long int *)&real_r_trachea_rc_filter_new[2] >> 24 & 0xFF;
  send_data_pack[index++] = *(unsigned long int *)&real_r_trachea_rc_filter_new[2] >> 16 & 0xFF;
  send_data_pack[index++] = *(unsigned long int *)&real_r_trachea_rc_filter_new[2] >> 8 & 0xFF;
  send_data_pack[index++] = *(unsigned long int *)&real_r_trachea_rc_filter_new[2] & 0xFF;

  send_data_pack[index++] = *(unsigned long int *)&real_r_trachea_rc_filter_new[3] >> 24 & 0xFF;
  send_data_pack[index++] = *(unsigned long int *)&real_r_trachea_rc_filter_new[3] >> 16 & 0xFF;
  send_data_pack[index++] = *(unsigned long int *)&real_r_trachea_rc_filter_new[3] >> 8 & 0xFF;
  send_data_pack[index++] = *(unsigned long int *)&real_r_trachea_rc_filter_new[3] & 0xFF;

  send_data_pack[index++] = *(unsigned long int *)&real_r_trachea_rc_filter_new[4] >> 24 & 0xFF;
  send_data_pack[index++] = *(unsigned long int *)&real_r_trachea_rc_filter_new[4] >> 16 & 0xFF;
  send_data_pack[index++] = *(unsigned long int *)&real_r_trachea_rc_filter_new[4] >> 8 & 0xFF;
  send_data_pack[index++] = *(unsigned long int *)&real_r_trachea_rc_filter_new[4] & 0xFF;

  send_data_pack[index++] = *(unsigned long int *)&real_r_trachea_rc_filter_new[5] >> 24 & 0xFF;
  send_data_pack[index++] = *(unsigned long int *)&real_r_trachea_rc_filter_new[5] >> 16 & 0xFF;
  send_data_pack[index++] = *(unsigned long int *)&real_r_trachea_rc_filter_new[5] >> 8 & 0xFF;
  send_data_pack[index++] = *(unsigned long int *)&real_r_trachea_rc_filter_new[5] & 0xFF;

  send_data_pack[index++] = *(unsigned long int *)&real_r_trachea_rc_filter_new[6] >> 24 & 0xFF;
  send_data_pack[index++] = *(unsigned long int *)&real_r_trachea_rc_filter_new[6] >> 16 & 0xFF;
  send_data_pack[index++] = *(unsigned long int *)&real_r_trachea_rc_filter_new[6] >> 8 & 0xFF;
  send_data_pack[index++] = *(unsigned long int *)&real_r_trachea_rc_filter_new[6] & 0xFF;

  send_data_pack[index++] = *(unsigned long int *)&real_r_trachea_rc_filter_new[7] >> 24 & 0xFF;
  send_data_pack[index++] = *(unsigned long int *)&real_r_trachea_rc_filter_new[7] >> 16 & 0xFF;
  send_data_pack[index++] = *(unsigned long int *)&real_r_trachea_rc_filter_new[7] >> 8 & 0xFF;
  send_data_pack[index++] = *(unsigned long int *)&real_r_trachea_rc_filter_new[7] & 0xFF;

  send_data_pack[index++] = *(unsigned long int *)&target_angle_x >> 24 & 0xFF;
  send_data_pack[index++] = *(unsigned long int *)&target_angle_x >> 16 & 0xFF;
  send_data_pack[index++] = *(unsigned long int *)&target_angle_x >> 8 & 0xFF;
  send_data_pack[index++] = *(unsigned long int *)&target_angle_x & 0xFF;
  //数组溢出也会导致程序失控

  send_data_pack[index++] = 0x55;

  HAL_UART_Transmit_DMA(&huart3, send_data_pack, index);  //不能放延时 中断里面
  adc_init();
}


void pc_data_deal()
{
    if(huart3_rev_data[0] == slave_rev_frame_head && huart3_rev_data[rev_pc_data_length - 1] == slave_rev_frame_tail) 
    {
        motor_type = huart3_rev_data[1];
        if(motor_type == slave_rev_RMD_Motor)
        {
            ctr_cmd = huart3_rev_data[2];
            ctr_ID = huart3_rev_data[3];
            if(rev_pc_data_length > 5)
            {
                //角度解析
                ctr_abs_ang = huart3_rev_data[4] << 8 | huart3_rev_data[5]; //上位机端已经作了角度限制，此处不用考虑
            }
        }
        else if(motor_type == slave_rev_Joint_Motor)
        {
            ctr_cmd = huart3_rev_data[2];
            ctr_ID = huart3_rev_data[3];
            if(rev_pc_data_length > 5)
            {
                ctr_abs_ang = huart3_rev_data[4] << 8 | huart3_rev_data[5];
            }
        }
        else if(motor_type == slave_rev_Mix_Motor)
        {
            Simulate_Human_Mode = huart3_rev_data[2];
        }
        else if(motor_type == Motor_Ctr_host_Command) //电机受控对象
        {
            PC_OR_HWT901_Ctr_Flag = huart3_rev_data[2]; // 0 HWT901 -- 1 PC
        }
        else if(motor_type == slave_rev_tendon)
        {
            tendon_Ctr_Flag = huart3_rev_data[2];
        }
        else if(motor_type == usb_imu_flag)
        {
            // 腕部：
            // fAcc[0] = *((float *)(&(unsigned int){huart3_rev_data[2] | huart3_rev_data[3] << 8 | huart3_rev_data[4] << 16 | huart3_rev_data[5] << 24}));
            // fAcc[1] = *((float *)(&(unsigned int){huart3_rev_data[6] | huart3_rev_data[7] << 8 | huart3_rev_data[8] << 16 | huart3_rev_data[9] << 24}));
            // fAcc[2] = *((float *)(&(unsigned int){huart3_rev_data[10] | huart3_rev_data[11] << 8 | huart3_rev_data[12] << 16 | huart3_rev_data[13] << 24}));

            // fGyro[0] = *((float *)(&(unsigned int){huart3_rev_data[14] | huart3_rev_data[15] << 8 | huart3_rev_data[16] << 16 | huart3_rev_data[17] << 24}));
            // fGyro[1] = *((float *)(&(unsigned int){huart3_rev_data[18] | huart3_rev_data[19] << 8 | huart3_rev_data[20] << 16 | huart3_rev_data[21] << 24}));
            // fGyro[2] = *((float *)(&(unsigned int){huart3_rev_data[22] | huart3_rev_data[23] << 8 | huart3_rev_data[24] << 16 | huart3_rev_data[25] << 24}));

            pitch = *((float *)(&(unsigned int){huart3_rev_data[26] | huart3_rev_data[27] << 8 | huart3_rev_data[28] << 16 | huart3_rev_data[29] << 24}));
            angle_x = roll = *((float *)(&(unsigned int){huart3_rev_data[30] | huart3_rev_data[31] << 8 | huart3_rev_data[32] << 16 | huart3_rev_data[33] << 24}));
            // yaw = *((float *)(&(unsigned int){huart3_rev_data[34] | huart3_rev_data[35] << 8 | huart3_rev_data[36] << 16 | huart3_rev_data[37] << 24}));

            // 肘部：
            // elbow_fAcc[0] = *((float *)(&(unsigned int){huart3_rev_data[38] | huart3_rev_data[39] << 8 | huart3_rev_data[40] << 16 | huart3_rev_data[41] << 24}));
            // elbow_fAcc[1] = *((float *)(&(unsigned int){huart3_rev_data[42] | huart3_rev_data[43] << 8 | huart3_rev_data[44] << 16 | huart3_rev_data[45] << 24}));
            // elbow_fAcc[2] = *((float *)(&(unsigned int){huart3_rev_data[46] | huart3_rev_data[47] << 8 | huart3_rev_data[48] << 16 | huart3_rev_data[49] << 24}));

            // elbow_fGyro[0] = *((float *)(&(unsigned int){huart3_rev_data[50] | huart3_rev_data[51] << 8 | huart3_rev_data[52] << 16 | huart3_rev_data[53] << 24}));
            // elbow_fGyro[1] = *((float *)(&(unsigned int){huart3_rev_data[54] | huart3_rev_data[55] << 8 | huart3_rev_data[56] << 16 | huart3_rev_data[57] << 24}));
            // elbow_fGyro[2] = *((float *)(&(unsigned int){huart3_rev_data[58] | huart3_rev_data[59] << 8 | huart3_rev_data[60] << 16 | huart3_rev_data[61] << 24}));

            elbow_pitch = *((float *)(&(unsigned int){huart3_rev_data[62] | huart3_rev_data[63] << 8 | huart3_rev_data[64] << 16 | huart3_rev_data[65] << 24}));
            elbow_angle = elbow_roll = *((float *)(&(unsigned int){huart3_rev_data[66] | huart3_rev_data[67] << 8 | huart3_rev_data[68] << 16 | huart3_rev_data[69] << 24}));
            elbow_yaw = *((float *)(&(unsigned int){huart3_rev_data[70] | huart3_rev_data[71] << 8 | huart3_rev_data[72] << 16 | huart3_rev_data[73] << 24}));

            wrist_trachea_diff = *((float *)(&(unsigned int){huart3_rev_data[74] | huart3_rev_data[75] << 8 | huart3_rev_data[76] << 16 | huart3_rev_data[77] << 24}));
        }
    }
}

void rev_pc_data_analysis(void)
{
  int32_t read_temp = 0;
  int16_t rev_pc_data_length_temp = 0;
//   UBaseType_t uxSavedInterruptStatus;
  if(__HAL_UART_GET_FLAG(&huart3, UART_FLAG_IDLE) != RESET)
  {
    // uxSavedInterruptStatus = taskENTER_CRITICAL_FROM_ISR();
    test_idle_uart3_count ++;
    __HAL_UART_CLEAR_FLAG(&huart3, UART_FLAG_IDLE);
    HAL_UART_DMAStop(&huart3); 
    read_temp = (&huart3)->Instance->SR;
    read_temp = (&huart3)->Instance->DR;
    rev_pc_data_length_temp = sizeof(huart3_rev_data) - (&hdma_usart3_rx)->Instance->NDTR;
    rev_pc_data_length = rev_pc_data_length_temp;
    pc_data_deal();
    // taskEXIT_CRITICAL_FROM_ISR(uxSavedInterruptStatus);
    memset(huart3_rev_data, sizeof(huart3_rev_data), 0);
    HAL_UART_Receive_DMA(&huart3, huart3_rev_data, sizeof(huart3_rev_data));
  }
}

void HAL_UART_TxCpltCallback(UART_HandleTypeDef *huart)
{
    if(__HAL_UART_GET_FLAG(&huart3, UART_FLAG_TC))
    {
        __HAL_UART_CLEAR_FLAG(&huart3, UART_FLAG_TC);
        osEventFlagsSet(Tx_Gui_eventHandle, tx_send_finish_event);
    }
}



